#include "ros/ros.h"
#include "std_msgs/String.h"
#include "std_msgs/Float64.h"
#include "std_msgs/UInt16.h"
#include <iostream>
#include <fstream>
#include <stdio.h>
#include <stdlib.h>
#include "geometry_msgs/TransformStamped.h"
#include "geometry_msgs/Transform.h"
#include "geometry_msgs/Vector3.h"
#include "tf/tfMessage.h"
#include <vector>
#include <math.h>
#include <limits>
#include "geometry_msgs/Twist.h"
#include "geometry_msgs/Pose.h"
#include <time.h>


using namespace std;

#include "Dictionary.cpp"
#include "Frame.cpp"
#include "FrameSet.cpp"
#include "Gesture.cpp"

ros::Publisher chatter_pub; // speech publisher
ros::Publisher angle_pub; // Kinect angle publisher
ros::Publisher led_pub; // Kinect LED publisher
ros::Publisher vel_pub; // robot base velocity publisher
ros::Publisher arm_pub; // robot arm pose publisher

Dictionary dict;
bool followHand = false;

geometry_msgs::Twist vel;

// RTAI test
//int poseCnt = 0;
//int twistCnt = 1000000;

void command(string command) {
	ROS_INFO("Command: %s", command.c_str());
	double x_vel = 1;
	double y_vel = 1;
	// double z_vel = 1; // not used
	// double rot_vel = 1; // not used
	
	std_msgs::String say;
	time_t t;
	time(&t);
    srand((unsigned int)t);
	
	if(!followHand) {
		// alternative solution: a map with strings as keys and ints or enums representing robot actions as values, then switch...
		if(command.compare("go left") == 0 || command.compare("move left") == 0) {
			vel.linear.y -= y_vel;
			say.data = "moving left";
			chatter_pub.publish(say);
		} else if(command.compare("go right") == 0 || command.compare("move right") == 0) {
			vel.linear.y += y_vel;
			say.data = "moving right";
			chatter_pub.publish(say);
		} else if(command.compare("go forward") == 0 || command.compare("move forward") == 0) {
			vel.linear.x += 4*x_vel;
			say.data = "moving forward";
			chatter_pub.publish(say);
		} else if(command.compare("go backward") == 0 || command.compare("move backward") == 0) {
			vel.linear.x -= x_vel;
			say.data = "moving backward";
			chatter_pub.publish(say);
		} else if(command.compare("Who are you") == 0 || command.compare("What is your name") == 0) {
			int n = 3;
			int rd = rand() % n + 1;
			switch(rd) {
				case 1: {
					say.data = "I am Hill The, the Hecksaabot.";
				} break;
				case 2: {
					say.data = "We are Her Royal Highness The Princess Hill The, Duchess of Linz";
				} break;
				case 3: {
					say.data = "Call me Hill The";
				} break;
			}
			chatter_pub.publish(say);
		} else if(command.compare("Are you a boy or a girl") == 0 || command.compare("Are you male or female") == 0) {
			say.data = "They did not assign me a gender yet.";
			chatter_pub.publish(say);
		} else if(command.compare("What's the time") == 0) {
			say.data = "It's half past time to buy a watch.";
			chatter_pub.publish(say);
		} else if(command.compare("I love you") == 0) {
			int n = 4;
			int rd = rand() % n + 1;
			switch(rd) {
				case 1: {
					say.data = "Will you still love me when I am old and rusty?";
				} break;
				case 2: {
					say.data = "You complete me";
				} break;
				case 3: {
					say.data = "I am not available, but I found twelve escorts fairly close to you.";
				} break;
				case 4: {
					say.data = "I am sorry, but I am in love with Wall E the Segway.";
				} break;
			}
			chatter_pub.publish(say);
		} else if(command.compare("Tell me a secret") == 0) {
			int n = 3;
			int rd = rand() % n + 1;
			switch(rd) {
				case 1: {
					say.data = "When I was a teenage robot, I had a crush on Kurt.";
				} break;
				case 2: {
					say.data = "Once I was hungry, I stole an apple from Alex. It was delicious.";
				} break;
				case 3: {
					say.data = "There are no secrets about me, I am open source.";
				} break;
			}
			chatter_pub.publish(say);
		} else if(command.compare("Play a song") == 0 || command.compare("Sing a song") == 0) {
			int n = 6;
			int rd = rand() % n + 1;
			switch(rd) {
				case 1: {
					say.data = "Lalalalalala";
				} break;
				case 2: {
					say.data = "I am sorry, I am not much of a singer.";
				} break;
				case 3: {
					system("mplayer /home/hilde/ros_workspace/hexabot/music/song1.ogg");
				} break;
				case 4: {
					system("mplayer /home/hilde/ros_workspace/hexabot/music/song2.ogg");
				} break;
				case 5: {
					system("mplayer /home/hilde/ros_workspace/hexabot/music/song3.ogg");
				} break;
				case 6: {
					system("mplayer /home/hilde/ros_workspace/hexabot/music/song4.ogg");
				} break;
			}
			chatter_pub.publish(say);
		} else if(command.compare("What is the best robot") == 0 || command.compare("Who is the best robot") == 0) {
			say.data = "Make an educated guess.";
			chatter_pub.publish(say);
		} else if(command.compare("Tell me a joke") == 0) {
			int n = 3;
			int rd = rand() % n + 1;
			switch(rd) {
				case 1: {
					say.data = "Two robots walk into a bar. I forgot the rest.";
				} break;
				case 2: {
					say.data = "I only know dirty jokes. I am afraid that I am not allowed to tell them.";
				} break;
				case 3: {
					say.data = "I don't know any.";
				} break;
			}
			chatter_pub.publish(say);
		} else if(command.compare("Who is your favorite actor") == 0) {
			say.data = "My favorite actor is Brent Spiner who played Data in Star Trek The Next Generation.";
			chatter_pub.publish(say);
		} else if(command.compare("I would like to know") == 0) {
			int n = 3;
			int rd = rand() % n + 1;
			switch(rd) {
				case 1: {
					say.data = "Did you know. That the average number of sesamee seeds. On a Big Mac bun is onehundred And eightytwo?";
				} break;
				case 2: {
					say.data = "Did you know. That there are 336 dimples on a regulation golf ball.";
				} break;
				case 3: {
					say.data = "Did you know. That the continents names. All end with the same letter with which they start.";
				} break;
			}
			chatter_pub.publish(say);
		} else if(command.compare("I need help") == 0) {
			say.data = "Talk to me. Make a gesture. Or show me a text command.";
			chatter_pub.publish(say);
		} else if(command.compare("follow my hand") == 0) {
			followHand = true;
			ROS_INFO("followHand ON");
			say.data = "Following your hand. Say stop action to switch back.";
			chatter_pub.publish(say);
		} // insert procedure for new command here
	}
	
	if(command.compare("stop action") == 0 || command.compare("stop") == 0) {
			followHand = false;
			vel.linear.x = 0.;
			vel.linear.y = 0.;
			vel.linear.z = 0.;
			vel.angular.x = 0.;
			vel.angular.y = 0.;
			vel.angular.z = 0.;
			ROS_INFO("followHand OFF");
			say.data = "stopping all actions";
			chatter_pub.publish(say);
	}
	
	vel_pub.publish(vel);
}

double getDistance(geometry_msgs::Vector3 v1, geometry_msgs::Vector3 v2) {
	return sqrt(pow(v2.x - v1.x, 2) + pow(v2.y - v1.y, 2) + pow(v2.z - v1.z, 2));
}

Frame rotFrame(Frame frame, double dx, double dy, double alpha) {
	double x = frame.getX() + dx;
	double y = frame.getY() + dy;
	double z = frame.getZ();
	
	double xn = x * cos(alpha) + y * sin(alpha);
	y = -x * sin(alpha) + y * cos(alpha);
	
	string name = frame.getName();
	geometry_msgs::Vector3 vec;
	vec.x = x;
	vec.y = y;
	vec.z = z;
	
	return Frame(name, vec);
}

void moveArm(FrameSet f) { // robot arm is moved by left human arm
	double alpha = atan2(f.left_shoulder.getX() - f.right_shoulder.getX() , f.right_shoulder.getY() - f.left_shoulder.getY());
	double dx = -(f.left_shoulder.getX() + f.right_shoulder.getX()) / 2; 
	double dy = -(f.left_shoulder.getY() + f.right_shoulder.getY()) / 2;
	Frame left_hand = rotFrame(f.left_hand, dx, dy, alpha);
	Frame left_elbow = rotFrame(f.left_elbow, dx, dy, alpha);
	Frame left_shoulder = rotFrame(f.left_shoulder, dx, dy, alpha);
	
	double x0 = left_shoulder.getX();
	double y0 = left_shoulder.getY() - 0.05; // more usable than 0
	double z0 = left_shoulder.getZ();
	double x = left_hand.getX();
	double y = left_hand.getY();
	double z = left_hand.getZ();
	
	geometry_msgs::Pose pose;
	pose.position.x = 0.;
	pose.position.y = 0.;
	pose.position.z = 0.;
	// quarternions (to be used in later projects)
	pose.orientation.x = 0.;
	pose.orientation.y = 0.;
	pose.orientation.z = 0.;
	pose.orientation.w = 0.;
	
	// since body measures obtained from Kinect are highly time-variant, a more stable but inexact guess for arm length is applied, source: http://de.wikipedia.org/wiki/K%C3%B6rperproportionen
	double humanArmLength = (left_shoulder.getZ() - (f.left_hip.getZ() + f.left_knee.getZ())/2); // avoid leaving the workspace
	if(!isnan(humanArmLength) || !isinf(humanArmLength)) { // avoid inaccurate or uninitialized Kinect data
		pose.position.x = (x - x0);
		pose.position.y = (y - y0);
		pose.position.z = (z - z0);
	}
		
	// RTAI test
	/*pose.position.x = poseCnt++;
	pose.position.y = poseCnt++;
	pose.position.z = poseCnt++;
	pose.orientation.x = poseCnt++;
	pose.orientation.y = poseCnt++;
	pose.orientation.z = poseCnt++;
	pose.orientation.w = poseCnt++;*/
	
	arm_pub.publish(pose);
}

void moveBase(FrameSet f) { // robot base is moved by right human arm
	double alpha = atan2(f.left_shoulder.getX() - f.right_shoulder.getX() , f.right_shoulder.getY() - f.left_shoulder.getY());
	double dx = -f.torso.getX();
	double dy = -f.torso.getY();
	
	// make it rotation-invariant
	Frame right_hand = rotFrame(f.right_hand, dx, dy, alpha);
	Frame right_elbow = rotFrame(f.right_elbow, dx, dy, alpha);
	Frame torso = rotFrame(f.torso, dx, dy, alpha);
	
	// human body-depending deadZone impossible because body measures (e.g. arm length) from Kinect are highly time-variant
	double deadZone = 0.05;
	double x0 = torso.getX() - 0.3; // x center is 0.3 m in front of torso
	double y0 = torso.getY() + 0.3; // y center is 0.3 m right of torso
	double x = right_hand.getX();
	double y = right_hand.getY();
	geometry_msgs::Twist vel;
	if(abs(x - x0) > deadZone) {
		if(x - x0 > 0) vel.linear.x = (x - x0 - deadZone)/deadZone;
		else vel.linear.x = (x - x0 + deadZone)/deadZone;
	} else vel.linear.x = 0;
	if(abs(y - y0) > deadZone) {
		if(y - y0 > 0) vel.linear.y = (int) ((y - y0 - deadZone)/deadZone);
		else vel.linear.y = (int) ((y - y0 + deadZone)/deadZone);
	} else vel.linear.y = 0;
	vel.linear.z = 0.;
	// rotational velocities (to used in later projects)
	vel.angular.x = 0.;
	vel.angular.y = 0.;
	vel.angular.z = 0.;
	
	// RTAI test
	/*vel.linear.x = twistCnt--;
	vel.linear.y = twistCnt--;
	vel.linear.z = twistCnt--;
	vel.angular.x = twistCnt--;
	vel.angular.y = twistCnt--;
	vel.angular.z = twistCnt--;*/
	
	vel_pub.publish(vel);
}

void readWordsCallback(const std_msgs::String readText)
{
	string str = readText.data;
	for(size_t i = 0; i < str.size(); i++) {
		str[i] = tolower(str[i]);
	}
	str = dict.findBestMatch(str);
	ROS_INFO("Read text: %s, matched to %s", readText.data.c_str(), str.c_str());
	if(str.length() != 0) command(str.c_str());
}

void heardWordsCallback(const std_msgs::String heardText)
{
	string str = dict.findBestMatch(heardText.data);
	ROS_INFO("Command heard: %s, matched to %s", heardText.data.c_str(), str.c_str());
	if(str.length() != 0) command(str.c_str());
}

FrameSet f = FrameSet();
Gesture g = Gesture();


Frame untiltFrame(Frame frame, double alpha) {
	double x = frame.getX();
	double y = frame.getY();
	double z = frame.getZ();
	
	double xn = x * cos(alpha) - z * sin(alpha);
	z = x * sin(alpha) + z * cos(alpha);
	
	string name = frame.getName();
	geometry_msgs::Vector3 vec;
	vec.x = x;
	vec.y = y;
	vec.z = z;
	
	return Frame(name, vec);
}

double kinectAngle = 0;

void gesturesCallback(const tf::tfMessage msg)
{
	int msgTime = msg.transforms[0].header.stamp.sec;
	size_t underlinePos = msg.transforms[0].child_frame_id.find_last_of('_');
	string frameType = msg.transforms[0].child_frame_id.substr(1, underlinePos - 1);
	int thisUserID = atoi(msg.transforms[0].child_frame_id.substr(underlinePos + 1, 1).c_str());
	
	f.updateActiveUser(thisUserID, msgTime);
	
	if(thisUserID == f.activeUserID) {
		// update FrameSet, switch left and right (input in Kinect POV)
		// acknowledging poor design of transforms struct
		if(frameType.compare("head") == 0) {
			f.head = untiltFrame(Frame(frameType, msg.transforms[0].transform.translation), kinectAngle);
		} else if(frameType.compare("neck") == 0) {
			f.neck = untiltFrame(Frame(frameType, msg.transforms[0].transform.translation), kinectAngle);
		} else if(frameType.compare("torso") == 0) {
			f.torso = untiltFrame(Frame(frameType, msg.transforms[0].transform.translation), kinectAngle);
		} else if(frameType.compare("left_shoulder") == 0) {
			f.right_shoulder = untiltFrame(Frame(frameType, msg.transforms[0].transform.translation), kinectAngle);
		} else if(frameType.compare("left_elbow") == 0) {
			f.right_elbow = untiltFrame(Frame(frameType, msg.transforms[0].transform.translation), kinectAngle);
		} else if(frameType.compare("left_hand") == 0) {
			f.right_hand = untiltFrame(Frame(frameType, msg.transforms[0].transform.translation), kinectAngle);
		} else if(frameType.compare("right_shoulder") == 0) {
			f.left_shoulder = untiltFrame(Frame(frameType, msg.transforms[0].transform.translation), kinectAngle);
		} else if(frameType.compare("right_elbow") == 0) {
			f.left_elbow = untiltFrame(Frame(frameType, msg.transforms[0].transform.translation), kinectAngle);
		} else if(frameType.compare("right_hand") == 0) {
			f.left_hand = untiltFrame(Frame(frameType, msg.transforms[0].transform.translation), kinectAngle);
		} else if(frameType.compare("left_hip") == 0) {
			f.right_hip = untiltFrame(Frame(frameType, msg.transforms[0].transform.translation), kinectAngle);
		} else if(frameType.compare("left_knee") == 0) {
			f.right_knee = untiltFrame(Frame(frameType, msg.transforms[0].transform.translation), kinectAngle);
		} else if(frameType.compare("left_foot") == 0) {
			f.right_foot = untiltFrame(Frame(frameType, msg.transforms[0].transform.translation), kinectAngle);
		} else if(frameType.compare("right_hip") == 0) {
			f.left_hip = untiltFrame(Frame(frameType, msg.transforms[0].transform.translation), kinectAngle);
		} else if(frameType.compare("right_knee") == 0) {
			f.left_knee = untiltFrame(Frame(frameType, msg.transforms[0].transform.translation), kinectAngle);
		} else if(frameType.compare("right_foot") == 0) {
			f.left_foot = untiltFrame(Frame(frameType, msg.transforms[0].transform.translation), kinectAngle);
		}
		
		// detect gesture
		if(g.inGestureArea(f) && !followHand) {
			string gesture = g.detectGesture(f, msgTime);
			if(gesture.length() != 0) command(gesture);
		}
		
		// follow hand?
		if(followHand) {
			moveArm(f);
			moveBase(f);
		}
	}
}


void kinectAngleCallback(const std_msgs::Float64 alpha) {
	kinectAngle = alpha.data;
}

void setKinectAngle(int angle) {
	// angle = -31..31
	std_msgs::Float64 f;
	f.data = angle;
	angle_pub.publish(f);
}

void setKinectLED(int col) {
	/* 0 - LED off
	 * 1 - green
	 * 2 - red
	 * 3 - orange
	 * 4 - flashing green
	 * 5 - flashing green
	 * 6 - flashing red and orange
	 * 7 - flashing red and orange
	*/
	std_msgs::UInt16 led;
	led.data = col;
	led_pub.publish(led);
}

void init() {
	setKinectLED(2);
	setKinectAngle(-31);
	sleep(2);
	setKinectLED(3);
	setKinectAngle(20); // final angle
	setKinectLED(1);
	std_msgs::String intro;
	intro.data = "Hello, I am Hill The. How can I help you";
	chatter_pub.publish(intro);
}

int main(int argc, char **argv) {
  
	ros::init(argc, argv, "main");
	
	ros::NodeHandle n;
	
	string dictFile;
	if(!ros::param::get("dictFile", dictFile)) {
		ROS_ERROR("Please specify dictFile parameter");
		ros::shutdown();
	}
	ROS_INFO("%d entries read from dictFile", dict.readDictionaryFile(dictFile));
	
	chatter_pub = n.advertise<std_msgs::String>("talker", 1000); // to hexabot_talker (speech synthesizer)
	angle_pub = n.advertise<std_msgs::Float64>("tilt_angle", 1000); // to kinect_aux (Kinect hardware)
	led_pub = n.advertise<std_msgs::UInt16>("led_option", 1000); // to kinect_aux (Kinect hardware)
	vel_pub = n.advertise<geometry_msgs::Twist>("twist", 1000); // to CB_TCP_RTAI (send to RTAI)
	arm_pub = n.advertise<geometry_msgs::Pose>("pose", 1000); // to CB_TCP_RTAI (send to RTAI)
	  
	ros::Subscriber subReadWords = n.subscribe("readWords", 1000, readWordsCallback); // from hexabot_readText (OCR)
	ros::Subscriber subheardWords = n.subscribe("hexabot_recognizer/output", 1000, heardWordsCallback); // from hexabot_recognizer (speech recognition)
	ros::Subscriber subGestures = n.subscribe("tf", 1000, gesturesCallback); // from openni_tracker (gesture recognition)
	ros::Subscriber subKinectAngle = n.subscribe("cur_tilt_angle", 1000, kinectAngleCallback); // from kinect_aux for untilting Kinect frames
	// binding variables together would avoid global variables
	//ros::Subscriber subGestures = n.subscribe("tf", 1000, boost::bind(gesturesCallback, _1, 1)); // does not work for unknown reasons
	
	sleep(1); // time to sync (ROS)
	
	// welcome tasks
	bool firstRun = true;
	if(firstRun) {
		init();
		firstRun = false;
	}
	
	ros::spin();
	return 0;
}
